package com.suray.rgv60.core;

import com.alibaba.fastjson.JSONArray;
import com.alibaba.fastjson.JSONException;
import com.alibaba.fastjson.JSONObject;
import com.suray.basic.wcs.path.core.Node;
import com.suray.basic.wcs.utils.LoggerType;
import com.suray.basic.wcs.utils.ModuleType;
import com.suray.basic.wcs.utils.WcsFuncUtil;
import com.suray.basic.wcs.utils.thread.BaseThread;
import com.suray.commin.core.utils.StringUtils;
import com.suray.commin.core.utils.UUID;
import com.suray.commin.exception.OrderExecuteException;
import com.suray.commin.exception.Rgv60CommFailException;
import com.suray.commin.exception.Rgv60InterruptException;
import com.suray.commin.exception.Rgv60TaskException;
import com.suray.rgv.data.Rgv60InfoData;
import com.suray.rgv.util.ErrorInfo;
import com.suray.rgv.util.RgvLogger;
import com.suray.rgv.util.RgvParamConfig;
import com.suray.rgv60.exception.ExecTaskException;
import com.suray.rgv60.manual.ManualMove;
import com.suray.rgv60.task.EndTaskManage;
import com.suray.rgv60.task.RgvTaskInfo;
import com.suray.rgv60.task.TaskInfo;
import com.suray.rgv60.util.*;
import com.suray.type.*;
import com.suray.wcs.res.enums.Order;
import suray.rgv.communication.SeerCommManage;
import suray.rgv.communication.data.AppiontRout;
import suray.rgv.communication.data.RouteInfo;
import suray.rgv.communication.data.status.CalibrationLocation;
import suray.rgv.communication.data.status.RobotTaskStatus;
import suray.rgv.communication.data.status.RobotTaskStatusRtn;
import suray.rgv.communication.util.RgvOperation;

import java.io.File;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;

/**
 * 两栖车核心
 */
public class Rgv60Core extends BaseThread {
    //通讯管理
    protected SeerCommManage seerComm;

    //已完成任务管理
    protected EndTaskManage endTaskManage;

    //当前的任务
    protected List<TaskInfo> tasks;

    //小车基本信息
    protected Rgv60Info rgv60Info;

    //序列号
    protected String serialNum;

    //内部时钟
    private int intervalTick = 0;

    //地图管理
    protected Rgv60MapManage mapManage;

    //手动行驶
    protected ManualMove manualMove;

    protected int rgvNo;

    public Rgv60Core(int rgvNo, String serialNum, ModuleType moduleType) {
        super(serialNum + "信息处理", RgvParamConfig.RGV_THREAD_TIME, ModuleType.RGV_RES60);
        this.serialNum = serialNum;
        this.intervalTick = 0;
        this.rgvNo = rgvNo;

        rgv60Info= new Rgv60Info(serialNum);

        seerComm = new SeerCommManage(serialNum, rgvNo);
        mapManage = new Rgv60MapManage(rgv60Info, seerComm);
        manualMove = new ManualMove(serialNum, seerComm);
    }

    protected Rgv60Info getRgv60Info() {
        return rgv60Info;
    }

    protected SeerCommManage getSeerComm() {
        return seerComm;
    }

    public String getSerialNum() {
        return serialNum;
    }

    /**
     * 初始化小车数据
     */
    public void initParam(boolean simpleInit) throws IOException {
        tasks = new ArrayList<>();
        endTaskManage = new EndTaskManage();
        rgv60Info.setRgvStatus(Rgv60Status.NONE);
        rgv60Info.initRgvInfo();
        if (!simpleInit && rgv60Info.hasRgvAuthor) {
            clearTask();
        }
    }

    public synchronized void init(String ip) throws IOException {
        seerComm.init(ip, rgv60Info.hasRgvAuthor);
        initParam(true);

        if (rgv60Info.rgvStatus == Rgv60Status.ERROR) {
            clearError();
        }
    }

    public void clearFault() throws IOException {
        seerComm.clearAllFault();
        rgv60Info.clearError();
        rgv60Info.clearInterrupt();
    }

    /**
     * 小车恢复
     * @return
     */
    public boolean recovery() throws IOException {
        RgvLogger.info(LoggerType.ERROR, null, rgv60Info.serialNum, "故障恢复", this.getClass());
        clearError();
        initParam(false);
        return true;
    }

    public Rgv60Status getRgvStatus() {
        return rgv60Info.getRgvStatus();
    }

    protected void setRgvStatus(Rgv60Status rgvStatus) {
        rgv60Info.setRgvStatus(rgvStatus);
    }

    public Node getLocation() {
        Node node = new Node(0,0,0);
        node.setRgv60Point(rgv60Info.currentStation);
        return node;
    }

    /**
     * 小车是否在线
     * @return
     */
    public boolean isOnLine() {
        return rgv60Info.onLine && seerComm.isOnLine();
//        return rgv60Info.onLine;
    }

    /**
     * 生成任务
     * @param taskId
     * @param nodeList
     */
    public void generalTask(Integer taskId, List<Node> nodeList) {
        checkAuthor();
        if (StringUtils.isEmpty(rgv60Info.currentStation)) {
            setRgvError(Rgv60ErrorUtil.NO_POSITION.getCode(), Rgv60ErrorUtil.NO_POSITION.getDes(),
                    Rgv60ErrorUtil.NO_POSITION.getLevel());
            throw new Rgv60TaskException(serialNum + "当前坐标为空，不能执行任务");
        }
        Node rgv60Point = nodeList.get(0);
        if (rgv60Point.getRgv60Point().equals(rgv60Info.currentStation)) {
            TaskInfo task = new TaskInfo(taskId, rgv60Point, nodeList, serialNum, RgvTaskType.RGV_60_TASK);
            tasks.add(task);
        } else {
            setRgvError(Rgv60ErrorUtil.TASK_START_NODE_FAULT.getCode(), Rgv60ErrorUtil.TASK_START_NODE_FAULT.getDes(),
                    Rgv60ErrorUtil.TASK_START_NODE_FAULT.getLevel());
            throw new Rgv60TaskException("起点位置与小车当前位置不一致：" + rgv60Info.currentStation + "， 整体路径:"
                    + WcsFuncUtil.listToString(nodeList));
        }
    }

    /**
     * 行驶任务
     * @param taskId
     * @param nodes
     * @return
     */
    public boolean task(Object taskId, List<Node> nodes) {
        checkAuthor();

        if (rgv60Info.rgvActualStatus == Rgv60Status.ERROR) {
            ErrorInfo errorInfo = rgv60Info.getHighestError();
            throw new OrderExecuteException(rgvNo, errorInfo.getErrorCode(), errorInfo.getErrorMessage());
        }

        for (TaskInfo task : tasks) {
            if (task.checkTaskId(taskId)) {
                //已存在任务
                task.generalTasks(nodes, RgvTaskType.RGV_60_TASK);
                return true;
            }
        }

        //新任务
//        List<Node> nodeList = new ArrayList<>();
//        Node node = new Node(0,0,0);
//        node.setRgv60Point(rgv60Info.currentStation);
//        nodeList.add(node);
//        nodeList.addAll(nodes);
//        TaskInfo task = new TaskInfo(taskId, nodeList, serialNum);
//        tasks.add(task);
//        return true;
        setRgvError(Rgv60ErrorUtil.NOT_EXIST_TASK.getCode(), Rgv60ErrorUtil.NOT_EXIST_TASK.getDes(),
                Rgv60ErrorUtil.NOT_EXIST_TASK.getLevel());
        throw new Rgv60TaskException("不存在该任务，无法执行动作：" + WcsFuncUtil.listToString(nodes));
    }

    /**
     * 任务是否结束
     * @param taskId
     * @return
     */
    public boolean isTaskEnd(Object taskId) {
        checkError();

        return endTaskManage.isTaskEnd(taskId);
    }

    protected void checkError() {
        if (rgv60Info.rgvActualStatus == Rgv60Status.ERROR) {
            setRgvStatus(Rgv60Status.ERROR);
            ErrorInfo errorInfo = getMaxError();
            if (rgv60Info.checkErrorLevel(errorInfo)) {
                throw new OrderExecuteException(rgvNo, errorInfo.getErrorCode(),
                        ", 异常原因：" + errorInfo.getErrorMessage());
            } else {
                throw new OrderExecuteException(rgvNo, Rgv60ErrorUtil.DEFAULT_ERROR.getCode(),
                        "小车上报故障，但是未上报具体的故障信息");
            }
        }
    }

    @Override
    protected void process() {
        try {
            if (rgv60Info.rgvStatus == null) {
                return;
            }

            if (!seerComm.robotStateOnLine()) {
                rgv60Info.onLine = false;
                return;
            }

            checkRgvStatus();

            if (rgv60Info.isHasRgvAuthor()) {
                analyzeTask();
                checkRobotTasks();
            }

            intervalTick++;
            if (intervalTick == 6000) {
                intervalTick = 0;
            }

            rgv60Info.onLine = true;
        } catch (IOException e) {
            RgvLogger.info(LoggerType.ERROR, null, serialNum, "离线", this.getClass());
            rgv60Info.onLine = false;
        } catch (Throwable e) {
            RgvLogger.info(LoggerType.ERROR, null, serialNum, "离线", this.getClass());
            RgvLogger.exceptionLog(LoggerType.COMM_DATA, null, serialNum, e, this.getClass());
            rgv60Info.onLine = false;
        }
    }

    /**
     * 获取小车控制权
     * @return
     * @throws IOException
     */
    private boolean lockRobot(String name) throws IOException {
        try {
            seerComm.lockRobot(name, 5);
            checkLockRobot();
            return rgv60Info.hasRgvAuthor;
        } catch (Rgv60CommFailException e) {
            rgv60Info.addError(new ErrorInfo(e.getCode(), e.getMsg(), ErrorLevel.ERROR_LEVEL_WCS));
            throw new Rgv60TaskException("异常码：" + e.getCode() + ", 故障描述：" + e.getMsg());
        }
    }

    /**
     * 获取小车控制权
     * @return
     * @throws IOException
     */
    private boolean unLockRobot() throws IOException {
        seerComm.releaseRobot();

        return true;
    }

    /**
     * 检测当前是否有控制权
     * @return
     * @throws IOException
     */
    protected boolean checkLockRobot() throws IOException {
        checkRobotInfo();
        return rgv60Info.hasRgvAuthor;
    }

    /**
     * 检测小车状态
     */
    protected void checkRgvStatus() throws IOException {
        checkRobotInfo();

        if (!isOnLine()) {
            setRgvStatus(Rgv60Status.OFF_LINE);
        }

        mapManage.analyzeMap();
        if (intervalTick % Rgv60CoreConfigParam.CHECK_ROBOT_MAP_TIME == 0) {
            checkLoadMaps();
        }
    }

    /**
     * 查询小车上面的地图状态
     */
    private void checkLoadMaps() throws IOException {
        JSONObject jsonObject = seerComm.checkRobotMaps();
        String currentMap = jsonObject.getString("current_map");
        JSONArray mapArray = jsonObject.getJSONArray("maps");
        for (Object o : mapArray) {
            rgv60Info.maps.add((String) o);
        }
    }

    /**
     * 查询机器人信息
     * @throws IOException
     */
    protected void checkRobotInfo() throws IOException {
        try {
//        RobotPositionInfo robotPositionInfo = seerComm.getRobotPositionInfo();
//        RobotPositioningStatus robotPositioningStatus = seerComm.getRobotPositionStatus();
            rgv60Info.analyzeRgv60Info(seerComm.getRobotInfo1());

            if (hasTask()) {
                if (rgv60Info.rgvActualStatus == Rgv60Status.STANDY) {
                    setRgvStatus(Rgv60Status.WAITING);
                }
            }
//            rgv60Info.readRobotIO(seerComm.checkRobotIO());

//        jsonObject.getJSONObject("current_lock").get("locked")
//        ((JSONObject)jsonObject.getJSONArray("lasers").get(0)).getJSONArray("beams")
        } catch (IOException e) {
            throw e;
        } catch (JSONException e) {
            RgvLogger.error(LoggerType.WARN, null, serialNum, "获取的小车基本信息不完整:" + e.getMessage(),
                    this.getClass());
        } catch (Throwable e) {
            RgvLogger.error(LoggerType.COMM_DATA, null, serialNum, "获取小车基本信息出错:" + e.getMessage(),
                    this.getClass());
            RgvLogger.exceptionLog(LoggerType.COMM_DATA, null, serialNum, e, this.getClass());
        }
    }

    /**
     * 查询任务状态
     * @throws IOException
     */
    private void checkRobotTasks() throws IOException {
        List<String> taskIds = new ArrayList<>();
        List<TaskInfoContainer> containers = new ArrayList<>();
        tasks.forEach(task -> {
            List<String> ids = task.getStartTasks();
            if (ids.size() > 0) {
                TaskInfoContainer container = new TaskInfoContainer(task, ids);
                containers.add(container);
                taskIds.addAll(ids);
            }
        });
        if (taskIds.size() > 0) {
            //有任务查询，没任务不查询
            freshTaskStatus(taskIds, containers);
        }
    }

    protected Rgv60MapManage getMapManage() {
        return mapManage;
    }

    public boolean action(Order rgvActionType, Object param) throws IOException {
        checkAuthor();
        switch (rgvActionType) {
            case MANUAL_CALIBRATION:
                calibration(rgvActionType, param);
                break;
            case CHANGE_LOCATION:
                return changeLocation(rgvActionType, param);
            case CHANGE_MAP:
                return changeMap(rgvActionType, param);
            case INITIALIZE:
                initialize(rgvActionType);
                break;
            case PALLET_DOWN:
                palletDown(rgvActionType);
                break;
            case PALLET_UP:
                palletUp(rgvActionType);
                break;
            case START_CHARGE:
                startCharge();
                break;
            case CLOSE_CHARGE:
                endCharge();
                break;
            default:
                RgvLogger.info(LoggerType.WARN, null, serialNum,
                        "目前不支持该指令:" + rgvActionType, this.getClass());
                break;
        }

        checkRobotInfo();

        return true;
    }

    /**
     * 结束充电
     */
    protected void endCharge() {

    }

    /**
     * 开始充电
     */
    protected void startCharge() {

    }

    /**
     * 托盘升
     * @param rgvActionType
     * @throws IOException
     */
    protected void palletUp(Order rgvActionType) throws IOException {
        RgvLogger.info(LoggerType.ACTION, rgvActionType.getOrderByte(), rgv60Info.serialNum, "托盘上升",
                this.getClass());
        if (!singleTask(null, RgvOperation.JackLoad.getStrValue(),
                null,"托盘上升", false)) {
            throw new Rgv60TaskException(serialNum + " 托盘上升失败");
        }

        if (rgv60Info.palletStatus != Rgv60PalletStatus.UP_OK) {
            throw new Rgv60TaskException("托盘上升执行完成，但是托盘状态不是顶升");
        }

        RgvLogger.info(LoggerType.ACTION, rgvActionType.getOrderByte(), rgv60Info.serialNum, "托盘上升成功",
                this.getClass());
    }

    /**
     * 托盘降
     * @param rgvActionType
     * @throws IOException
     */
    protected void palletDown(Order rgvActionType) throws IOException {
        RgvLogger.info(LoggerType.ACTION, rgvActionType.getOrderByte(), rgv60Info.serialNum, "托盘下降",
                this.getClass());
        if (!singleTask(null, RgvOperation.JackUnLoad.getStrValue(),
                null, "托盘下降", false)) {
            throw new Rgv60TaskException(serialNum + " 托盘下降失败");
        }

        if (rgv60Info.palletStatus != Rgv60PalletStatus.DOWN_OK) {
            throw new Rgv60TaskException("托盘下降执行完成，但是托盘状态不是下降");
        }

        RgvLogger.info(LoggerType.ACTION, rgvActionType.getOrderByte(), rgv60Info.serialNum, "托盘下降成功",
                this.getClass());
    }

    /**
     * 初始化
     * @param rgvActionType
     * @throws IOException
     */
    protected void initialize(Order rgvActionType) throws IOException {
        RgvLogger.info(LoggerType.ACTION, rgvActionType.getOrderByte(), rgv60Info.serialNum, "初始化",
                this.getClass());
        initParam(false);
    }

    /**
     * 切换地图
     * @param rgvActionType
     * @param param
     * @return
     * @throws IOException
     */
    protected boolean changeMap(Order rgvActionType, Object param) throws IOException {
        RgvLogger.info(LoggerType.ACTION, rgvActionType.getOrderByte(), rgv60Info.serialNum, "切换地图",
                this.getClass());
        if (!changeMap((Integer) param)) {
            return false;
        }

        return true;
    }

    /**
     * 修改位置
      * @param rgvActionType
     * @param param
     * @return
     * @throws IOException
     */
    protected boolean changeLocation(Order rgvActionType, Object param) throws IOException {
        RgvLogger.info(LoggerType.ACTION, rgvActionType.getOrderByte(), rgv60Info.serialNum, "修改位置",
                this.getClass());
        if (!changeLocation((String) param)) {
            return false;
        }

        return true;
    }

    /**
     * 校准位置
     * @param rgvActionType
     * @param o
     */
    protected void calibration(Order rgvActionType, Object o) throws IOException {
        RgvLogger.info(LoggerType.ACTION, rgvActionType.getOrderByte(), rgv60Info.serialNum, "手动校准",
                this.getClass());
        mapManage.confirmRobotPosition();
    }

    /**
     * 修改小车位置:小车换层，切换地图
     * @param layer:层
     * @return
     * @throws IOException
     */
    public boolean changeMap(Integer layer) throws IOException {
//        checkAuthor();
        mapManage.changeMap(layer);
        return true;
    }

    /**
     * 修改小车位置
     * @return
     * @param param:层数
     */
    protected boolean changeLocation(String param) throws IOException {
        checkAuthor();
        return singleTask(param, null, null, "修改小车位置", false);
    }

    private boolean singleTask(String targetNode, String operation, String direction, String des, boolean isScript)
            throws IOException {
        RouteInfo routeInfo = new RouteInfo();
        String taskId = UUID.randomUUID().toString();
        routeInfo.setTask_id(taskId);
        if (StringUtils.isNotEmpty(targetNode)) {
            routeInfo.setId(targetNode);
        }
        if (isScript) {
            routeInfo.setOperation("Script");
            RouteInfo.ScriptDTO scriptDTO = new RouteInfo.ScriptDTO();
            scriptDTO.setDirection(direction);
            scriptDTO.setOperation(operation);
            routeInfo.setScript_args(scriptDTO);
            routeInfo.setScript_name("userpy/SingleRoller.py");
        } else {
            if (StringUtils.isNotEmpty(operation)) {
                routeInfo.setOperation(operation);
            }
            if (StringUtils.isNotEmpty(direction)) {
                routeInfo.setDirection(direction);
            }
        }
        seerComm.sendRoute(routeInfo);

        int tick = 150;
        while (true) {
            checkRgvStatus();
            if (rgv60Info.taskStatus == Rgv60TaskStatus.COMPLETED) {
                if (operation != null) {
                    if ((operation.equals(RgvOperation.JackLoad.getStrValue())
                            && rgv60Info.palletStatus == Rgv60PalletStatus.UP_OK
                        )
                        || (operation.equals(RgvOperation.JackUnLoad.getStrValue())
                            && rgv60Info.palletStatus == Rgv60PalletStatus.DOWN_OK
                        )
                    ) {
                        break;
                    }
                } else {
                    break;
                }
            }

            rgvInterrupt(des);
            if (tick > 0) {
                tick--;
            } else {
                return false;
            }
            WcsFuncUtil.delay(200);
        }

        return true;
    }

    /**
     * 刷新任务状态
     * @param taskIds
     * @param containers
     */
    private void freshTaskStatus(List<String> taskIds, List<TaskInfoContainer> containers) throws IOException {
        RobotTaskStatusRtn robotTask = new RobotTaskStatusRtn();
        robotTask.setTaskIds(taskIds);
        RobotTaskStatus robotTaskStatus = seerComm.getRobotTaskStatus(robotTask);

        robotTaskStatus.getTask_status_package().getTask_status_list().forEach(taskStatus -> {
            for (TaskInfoContainer container : containers) {
                if (container.rgvTaskIds.contains(taskStatus.getTask_id())) {
                    try {
                        container.task.freshTaskStatus(taskStatus.getTask_id(), taskStatus.getStatus());
                    } catch (ExecTaskException e) {
                        setRgvError(Rgv60ErrorUtil.PERFORM_TASK_ERROR.getCode(),
                                Rgv60ErrorUtil.PERFORM_TASK_ERROR.getDes(),
                                Rgv60ErrorUtil.PERFORM_TASK_ERROR.getLevel());
                    }
                    break;
                }
            }
        });

    }

    /**
     * 小车中断
     */
    protected void rgvInterrupt(String des) {
        if (rgv60Info.isInterrupt()) {
            throw new Rgv60InterruptException(serialNum + ":" + des + " 中断");
        }
        if (rgv60Info.rgvActualStatus == Rgv60Status.ERROR) {
            throw new Rgv60TaskException(serialNum + ":" + getMaxError().getErrorMessage());
        }
    }

    /**
     * 清除故障
     */
    public void clearError() throws IOException {
        checkAuthor();
        clearTask();
        seerComm.clearAllFault();
        /**
         * TODO 重定位放在这里会需要两次点击故障恢复才能恢复故障
         * TODO 后面需找小车测试把重定位放到最下面执行
         */
        if (StringUtils.isNotEmpty(rgv60Info.robotCurrentStation)) {
            if (!changeLocation(rgv60Info.robotCurrentStation)) {
                throw new Rgv60TaskException("小车重定位失败");
            }
        }
        rgv60Info.clearError();
        rgv60Info.clearInterrupt();

        checkRobotInfo();
        if (rgv60Info.rgvActualStatus == Rgv60Status.ERROR) {
            throw new Rgv60TaskException("清除故障失败");
        }
    }

    /**
     * 清除路径导航
     * @throws IOException
     */
    public void clearTask() throws IOException {
        seerComm.clearTask();
    }

    /**
     * 设置中断
     */
    public void setInterrupt() throws IOException {
        rgv60Info.setInterrupt();
        rgvError(new ErrorInfo(Rgv60ErrorUtil.INTERRUPT_RGV.getCode(), Rgv60ErrorUtil.INTERRUPT_RGV.getDes(),
                Rgv60ErrorUtil.INTERRUPT_RGV.getLevel()));
        checkAuthor();
        seerComm.setRobotError("中断小车");
    }

    public ErrorInfo getMaxError() {
        return rgv60Info.getHighestError();
    }

    public List<ErrorInfo> getErrors() {
        return rgv60Info.getErrors();
    }

    public void removeError(ErrorInfo errorInfo) {
        rgv60Info.deleteErrorInfo(errorInfo);
    }

    protected void setPalletStatus(Rgv60PalletStatus palletStatus) {
        rgv60Info.palletStatus = palletStatus;
    }

    public Rgv60PalletStatus getPalletStatus() {
        return rgv60Info.palletStatus;
    }

    /**
     * 检测是否有小车控制权
     */
    protected void checkAuthor() {
        if (!rgv60Info.isHasRgvAuthor()) {
            throw new Rgv60TaskException(serialNum + " 当前没有小车控制权");
        }
    }

    public Boolean hasPallet() {
        return rgv60Info.hasCargo;
    }

    /**
     * 获取小车信息
     * @return
     */
    public Rgv60InfoData getRgv60InfoData() {
        Rgv60InfoData rgv60InfoData = new Rgv60InfoData();
        rgv60InfoData.setCurrentStation(rgv60Info.currentStation);
        rgv60InfoData.setRelocStatus(rgv60Info.relocStatus);
        rgv60InfoData.setRobotCurrentStation(rgv60Info.robotCurrentStation);
        rgv60InfoData.setLastStation(rgv60Info.lastStation);
        rgv60InfoData.setSerialNum(rgv60Info.getSerialNum());
        rgv60InfoData.setOnLine(rgv60Info.onLine);
        rgv60InfoData.setRouteOnLine(seerComm.routeOnLine());
        rgv60InfoData.setConfigOnLine(seerComm.configOnLine());
        rgv60InfoData.setControlOnLine(seerComm.controlOnLine());
        rgv60InfoData.setRobotStateOnLine(seerComm.robotStateOnLine());
        rgv60InfoData.setHasRgvAuthor(rgv60Info.hasRgvAuthor);
        rgv60InfoData.setRgvStatus(rgv60Info.rgvStatus);
        rgv60InfoData.setRgvActStatus(rgv60Info.rgvActualStatus);
        rgv60InfoData.setErrorInfos(rgv60Info.getErrors());
        rgv60InfoData.setHighestError(rgv60Info.getHighestError() == null ?
                null : rgv60Info.getHighestError().getErrorMessage());
        rgv60InfoData.setPalletStatus(rgv60Info.palletStatus);
        rgv60InfoData.setPalletEnable(rgv60Info.palletEnable);
        rgv60InfoData.setPalletActionErrorCode(rgv60Info.palletActionErrorCode);
        rgv60InfoData.setHasCargo(rgv60Info.hasCargo);
        rgv60InfoData.setX(rgv60Info.x);
        rgv60InfoData.setY(rgv60Info.y);
        rgv60InfoData.setAngle(rgv60Info.angle);
        rgv60InfoData.setConfidence(rgv60Info.confidence);
        rgv60InfoData.setBlocked(rgv60Info.blocked);
        rgv60InfoData.setBatSoc(rgv60Info.batSoc);
        rgv60InfoData.setBatTemp(rgv60Info.batTemp);
        rgv60InfoData.setBatVolt(rgv60Info.batVolt);
        rgv60InfoData.setBatCur(rgv60Info.batCur);
        rgv60InfoData.setAutoCharge(rgv60Info.autoCharge);
        rgv60InfoData.setCharging(rgv60Info.charging);
        rgv60InfoData.setTaskStatus(rgv60Info.taskStatus);
        rgv60InfoData.setTargetStation(rgv60Info.targetStation);
        rgv60InfoData.setTaskType(rgv60Info.taskType);
        rgv60InfoData.setLoadMapStatus(rgv60Info.loadMapStatus);
        rgv60InfoData.setMode(rgv60Info.mode);
        rgv60InfoData.setRollerMode(rgv60Info.rollerMode);
        rgv60InfoData.setCurrentMap(rgv60Info.currentMap);
        rgv60InfoData.setCurrentMapMD5(rgv60Info.currentMapMD5);
        rgv60InfoData.setWifiStrength(rgv60Info.wifiStrength);
        rgv60InfoData.setRobotName(rgv60Info.robotName);
        rgv60InfoData.setMaps(new ArrayList<>(rgv60Info.maps));
        rgv60InfoData.setOdo(rgv60Info.odo);
        rgv60InfoData.setTodayOdo(rgv60Info.todayOdo);
        rgv60InfoData.setRunningTime(rgv60Info.runningTime);
        rgv60InfoData.setCurRunningTime(rgv60Info.curRunningTime);
        rgv60InfoData.setControlVolt(rgv60Info.controlVolt);
        rgv60InfoData.setControlTemp(rgv60Info.controlTemp);
        rgv60InfoData.setControlWet(rgv60Info.controlWet);
        rgv60InfoData.setChargeNum(rgv60Info.chargeNum);
        rgv60InfoData.setPermitMaxChgVolt(rgv60Info.permitMaxChgVolt);
        rgv60InfoData.setPermitMaxChgCur(rgv60Info.permitMaxChgCur);
        rgv60InfoData.setBlockX(rgv60Info.blockX);
        rgv60InfoData.setBlockY(rgv60Info.blockY);
        rgv60InfoData.setDriveEmc(rgv60Info.driveEmc);
        rgv60InfoData.setHoldBrake(rgv60Info.holdBrake);
        rgv60InfoData.setIsStop(rgv60Info.isStop);
        rgv60InfoData.setTargetX(rgv60Info.targetX);
        rgv60InfoData.setTargetY(rgv60Info.targetY);
        rgv60InfoData.setTargetR(rgv60Info.targetR);
        rgv60InfoData.setRobotNote(rgv60Info.robotNote);

        Map<RobotIOType.ROBOT_DI_TYPE, Boolean> DIs = rgv60Info.getDIs();
        Map<RobotIOType.ROBOT_DO_TYPE, Boolean> DOs = rgv60Info.getDOs();

        DIs.keySet().forEach(DI -> {
            rgv60InfoData.getDI()[DI.getValue()] = DIs.get(DI);
        });

        DOs.keySet().forEach(DO -> {
            rgv60InfoData.getDO()[DO.getValue()] = DOs.get(DO);
        });

        return rgv60InfoData;
    }

    public String getRobotName() {
        return rgv60Info.robotName;
    }

    public boolean emergencyStop() {
        return rgv60Info.driveEmc;
    }

    public String getMap() {
        return rgv60Info.currentMap;
    }

    public Integer getChargeNum() {
        return rgv60Info.chargeNum;
    }

    public Float getPermitChargeVolt() {
        return rgv60Info.permitMaxChgVolt;
    }

    public Object getTaskId() {
        return tasks.get(0).getTaskId();
    }

    public Integer getCurrentFloor() {
        return mapManage.getCurrentFloor();
    }

    /**
     * 修改权限
     * @param hasAuthor
     *
     */
    public void changeAuthor(boolean hasAuthor) throws IOException {
        if (hasAuthor) {
            if (!lockRobot(serialNum)) {
                throw new Rgv60TaskException("获取控制权失败");
            }
        } else {
            unLockRobot();
        }
    }

    public boolean hasAuthor() {
        return rgv60Info.hasRgvAuthor;
    }

    public void resetPosition() throws IOException {
        if (StringUtils.isEmpty(rgv60Info.robotCurrentStation)) {
            throw new Rgv60TaskException("小车不在码点上，无法重定位");
        }

        CalibrationLocation calibrationLocation = new CalibrationLocation();
        calibrationLocation.setX(rgv60Info.x);
        calibrationLocation.setY(rgv60Info.y);
        calibrationLocation.setAngle(rgv60Info.angle / 180 * Rgv60CoreConfigParam.PI);
        seerComm.calibrationLocation(calibrationLocation);
    }

    public void sendMap(List<File> files) throws IOException {
//        for (File file : files) {
//            mapManage.sendMap(file.getPath());
//        }
    }

    public void initNet(String ip) {
        seerComm.initNet(ip);
    }

    /**
     * 下载指定地图
     * @param mapName
     * @return
     * @throws IOException
     */
    public String downloadMap(String mapName) throws IOException {
        return mapManage.downloadMap(mapName);
    }

    /**
     * 删除指定地图
     * @param mapName
     * @throws IOException
     */
    public void deleteMap(String mapName) throws IOException {
        mapManage.deleteMap(mapName);
    }

    /**
     * 前进
     */
    public void forwardRun() {
        manualMove.changeManualMove(ManualMoveType.FORWARD);
    }

    /**
     * 后退
     */
    public void fallBackRun() {
        manualMove.changeManualMove(ManualMoveType.FALL_BACK);
    }

    /**
     * 左转
     */
    public void turnLeft() {
        manualMove.changeManualMove(ManualMoveType.TURN_LEFT);
    }

    /**
     * 右转
     */
    public void turnRight() {
        manualMove.changeManualMove(ManualMoveType.TURN_RIGHT);
    }

    /**
     * 停止手动行驶
     */
    public void stopManualMove() {
        manualMove.changeManualMove(ManualMoveType.STOP_MOVE);
    }

    /**
     * 小车状态变为error
     */
    public void rgvError(ErrorInfo errorInfo) {
        setRgvStatus(Rgv60Status.ERROR);
        rgv60Info.addError(errorInfo);
    }

    public boolean isCharging() {
        return rgv60Info.charging;
    }

    /**
     * 车辆是否是静止的
     * @return
     */
    public boolean isStatic() {
        return rgv60Info.getRgvStatus() != Rgv60Status.RUNNING;
    }

    public boolean positionOk() {
        return rgv60Info.relocStatus != SeerRobotPositionStatus.SUCCESS;
    }

    class TaskInfoContainer {
        TaskInfo task;
        List<String> rgvTaskIds;

        TaskInfoContainer(TaskInfo task, List<String> rgvTaskIds) {
            this.task = task;
            this.rgvTaskIds = rgvTaskIds;
        }
    }

    /**
     * 任务处理
     */
    private void analyzeTask() throws IOException {
        //无任务
        if (tasks.size() == 0) {
            return;
        }

        //故障状态
        if (rgv60Info.getRgvStatus().equals(Rgv60Status.ERROR)) {
            return;
        }

        try {
            TaskInfo task = tasks.get(0);
            if (!task.checkTaskEnd(rgv60Info.currentStation)) {
                doTask(task);
            } else {
                endTask(task);
            }
        } catch (ExecTaskException e) {
            setRgvStatus(Rgv60Status.ERROR);
            setRgvError(Rgv60ErrorUtil.PERFORM_TASK_ERROR.getCode(),
                    e.getMessage(),
                    Rgv60ErrorUtil.PERFORM_TASK_ERROR.getLevel());
        }
    }

    /**
     * 结束任务
     * @param task
     */
    protected void endTask(TaskInfo task) {
        endTaskManage.addEndTask(task, System.currentTimeMillis());
        tasks.remove(task);
    }

    public void setRgvError(int errorCode, String str, ErrorLevel errorLevel) {
        rgv60Info.addError(new ErrorInfo(errorCode, str, errorLevel));
        RgvLogger.info(LoggerType.TASK, errorLevel.getValue(), serialNum, str, this.getClass());
//        rgv60Info.rgvStatus = Rgv60Status.ERROR;
    }

    /**
     * 当前是否有任务待执行
     * @return
     */
    public boolean hasTask() {
        return tasks.size() > 0;
    }

    /**
     * 下发任务
     * @param task
     */
    protected void doTask(TaskInfo task) throws IOException {
        RgvTaskInfo rgvTask = task.getNextTask();
        List<AppiontRout.MoveTaskListDTO> moveTaskListDTOS = new ArrayList<>();
        StringBuffer sb = new StringBuffer();

        if (rgv60Info.rgvActualStatus == Rgv60Status.ERROR) {
            rgv60Info.setRgvStatus(Rgv60Status.ERROR);
        }

        while (rgvTask != null) {
            Node targetNode = rgvTask.getEndNode();
            sb.append(rgvTask.toString());
            AppiontRout.MoveTaskListDTO moveTaskListDTO = new AppiontRout.MoveTaskListDTO();
            moveTaskListDTO.setTask_id(rgvTask.getRgvTaskNo());
            moveTaskListDTO.setSource_id(rgvTask.getStartNode().getRgv60Point());
            moveTaskListDTO.setId(targetNode.getRgv60Point());
            analyzeNodeAction(moveTaskListDTO, rgvTask.getStartNode(), rgvTask.getEndNode());
            analyzeScript(moveTaskListDTO, targetNode);
            moveTaskListDTOS.add(moveTaskListDTO);
            rgvTask = task.getNextTask();
        }

        if (moveTaskListDTOS.size() > 0) {
            RgvLogger.info(LoggerType.TASK, task.getTaskId(), serialNum, sb.toString(), this.getClass());
            seerComm.sendAppointRout(moveTaskListDTOS);
        }
    }

    /**
     * 解析是否需要脚本
     * @param moveTaskListDTO
     * @param targetNode
     */
    private void analyzeScript(AppiontRout.MoveTaskListDTO moveTaskListDTO, Node targetNode) {
        if (StringUtils.isEmpty(targetNode.getScript())) {
            return;
        }

        moveTaskListDTO.setOperation("Script");
        moveTaskListDTO.setScript_name("userpy/script-SURAY-v3.py");
        moveTaskListDTO.setScript_stage(2);
        moveTaskListDTO.setScript_args(targetNode.getScript());
    }

    /**
     * 添加节点动作
     * @param moveTaskListDTO
     * @param startNode
     * @param endNode
     */
    protected void analyzeNodeAction(AppiontRout.MoveTaskListDTO moveTaskListDTO, Node startNode, Node endNode) {

    }

    public Float getBatSoc() {
        return rgv60Info.batSoc == null ? 0 : rgv60Info.batSoc * 100;
    }

    public Float getBatTemp() {
        return rgv60Info.batTemp == null ? 0 : rgv60Info.batTemp;
    }

    public Float getBatVolt() {
        return rgv60Info.batVolt;
    }

    public Float getBatCur() {
        return rgv60Info.batCur;
    }

    @Override
    public String toString() {
        return serialNum;
    }

    @Override
    public void endThread() {
        seerComm.destroy();
        super.endThread();
    }
}
